#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <fstream>
#include <Eigen/Dense>
#include <pcl/common/transforms.h>

// 定义文件保存路径（根据实际工作空间修改）
const std::string LASER_TRAJ_PATH = "/home/ros-demo/作业/里程计实验作业/lio_icp/src/lio_ndt/trajectories/laser_odom.txt";
const std::string GNSS_TRAJ_PATH = "/home/ros-demo/作业/里程计实验作业/lio_icp/src/lio_ndt/trajectories/gnss_ground_truth.txt";

// 激光里程计回调函数：保存轨迹到文件
void LaserOdomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) {
    static std::ofstream laser_file(LASER_TRAJ_PATH, std::ios::out);
    if (!laser_file.is_open()) {
        ROS_ERROR("Failed to open laser trajectory file!");
        return;
    }
    // TUM格式：时间戳 x y z qx qy qz qw
    laser_file << std::fixed << std::setprecision(9)
               << odom_msg->header.stamp.toSec() << " "
               << odom_msg->pose.pose.position.x << " "
               << odom_msg->pose.pose.position.y << " "
               << odom_msg->pose.pose.position.z << " "
               << odom_msg->pose.pose.orientation.x << " "
               << odom_msg->pose.pose.orientation.y << " "
               << odom_msg->pose.pose.orientation.z << " "
               << odom_msg->pose.pose.orientation.w << std::endl;
}

// GNSS真值回调函数：保存轨迹到文件
void GnssOdomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) {
    static std::ofstream gnss_file(GNSS_TRAJ_PATH, std::ios::out);
    if (!gnss_file.is_open()) {
        ROS_ERROR("Failed to open GNSS trajectory file!");
        return;
    }
    // TUM格式：时间戳 x y z qx qy qz qw
    gnss_file << std::fixed << std::setprecision(9)
              << odom_msg->header.stamp.toSec() << " "
              << odom_msg->pose.pose.position.x << " "
              << odom_msg->pose.pose.position.y << " "
              << odom_msg->pose.pose.position.z << " "
              << odom_msg->pose.pose.orientation.x << " "
              << odom_msg->pose.pose.orientation.y << " "
              << odom_msg->pose.pose.orientation.z << " "
              << odom_msg->pose.pose.orientation.w << std::endl;
}

int main(int argc, char**argv) {
    ros::init(argc, argv, "trajectory_saver_node");
    ros::NodeHandle nh;

    // 创建保存目录（若不存在）
    std::string traj_dir = "/home/ros-demo/作业/里程计实验作业/lio_icp/src/lio_ndt/trajectories";
    system(("mkdir -p " + traj_dir).c_str());

    // 订阅激光里程计（话题名与front_end_node发布的一致）
    ros::Subscriber laser_sub = nh.subscribe("/laser_odom", 100, LaserOdomCallback);
    // 订阅GNSS真值（话题名与front_end_node发布的一致）
    ros::Subscriber gnss_sub = nh.subscribe("/gnss", 100, GnssOdomCallback);

    ROS_INFO("Trajectory saver node started. Saving to: %s", traj_dir.c_str());
    ros::spin();

    return 0;
}